﻿using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;

namespace JupiterLibrary
{
    public class TCollisionPacket
    {

        // data about player movement
        public Vector3 velocity;
        public Vector3 sourcePoint;

        // radius of ellipsoid.  
        //D3DXVECTOR3 eRadius;

        // for error handling  
        public Vector3 lastSafePosition;
        public bool stuck;

        // data for collision response 
        public bool foundCollision;
        public float nearestDistance; // nearest distance to hit
        /// <summary>
        /// on sphere
        /// </summary>
        public Vector3 nearestIntersectionPoint; 
        public Vector3 nearestPolygonIntersectionPoint; // on polygon
    };

    public class Triangle3
    {
        public Vector3 A, B, C;

        public Plane Plane;
    }
#if Javier
    public class Collision
    {
        /// <summary>
        /// Lista de triángulos que se comprueba con la colisión
        /// </summary>
        public List<Triangle3> Triangles;

        /// <summary>
        /// Información sobre el resultado de la colisión
        /// </summary>
        public TCollisionPacket colPackage = new TCollisionPacket();

        /// <summary>
        /// Radio de la elipsoide
        /// </summary>
        public Vector3 eRadius = new Vector3(1, 1, 1);

        public bool HayColision;

        /// <summary>
        /// interseccion de rayo con Plano. se esperan vectores direccionales unitarios
        /// </summary>
        /// <param name="rOrigin">origen del rayo</param>
        /// <param name="rVector">direccion del rayo</param>
        /// <param name="pOrigin">origen del plano</param>
        /// <param name="pNormal">normal al plano</param>
        /// <returns>distancia al plano, -1 si no hay interseccion</returns>
        public float IntersectRayPlane(Vector3 rOrigin, Vector3 rVector, Vector3 pOrigin, Vector3 pNormal)
        {
            float d = -Vector3.Dot(pNormal, pOrigin);

            float numer = Vector3.Dot(pNormal, rOrigin) + d;
            float denom = Vector3.Dot(pNormal, rVector);

            if (denom == 0) return -1;

            return -(numer / denom);
        }

        /// <summary>
        /// interseccion de rayo con Plano
        /// </summary>
        /// <param name="ray"></param>
        /// <param name="plane"></param>
        /// <returns>distancia al plano, -1 si no hay interseccion</returns>
        public float IntersectRayPlane(Ray ray, Plane plane)
        {
            float? d = ray.Intersects(plane);

            if (d == null) return -1;

            return d.Value;
        }

        /// <summary>
        /// interseccion de un rayo en una esfera
        /// </summary>
        /// <param name="ray"></param>
        /// <param name="sphere"></param>
        /// <returns>distancia a la esfera, -1 si no hay interseccion</returns>
        public float IntersectRaySphere(Ray ray, BoundingSphere sphere)
        {
            float? d = ray.Intersects(sphere);

            if (d == null) return -1;

            return d.Value;
        }

        /// <summary>
        /// interseccion de un rayo en una esfera. se esperan vectores direccionales unitarios (normalizados)
        /// </summary>
        /// <param name="rO">origen del rayo</param>
        /// <param name="rV">direccion del rayo</param>
        /// <param name="sO">centro de la esfera</param>
        /// <param name="sR">radio de la esfera</param>
        /// <returns>distancia a la esfera, -1 si no hay interseccion</returns>
        public float IntersectRaySphere(Vector3 rO, Vector3 rV, Vector3 sO, float sR)
        {
            Vector3 Q = sO - rO;

            float c = Q.Length();
            float v = Vector3.Dot(Q, rV);
            float d = sR * sR - (c * c - v * v);

            // If there was no intersection, return -1
            if (d < 0.0f) return (-1.0f);

            // Return the distance to the [first] intersecting point
            return (v - (float)Math.Sqrt(d));
        }

        /// <summary>
        /// chequea si un punto esta dentro de un triangulo. el triangulo deberia estar definido en orden horario, abc
        /// </summary>
        /// <param name="point"></param>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="c"></param>
        /// <returns></returns>
        public bool CheckPointInTriangleNormal(Vector3 point, Vector3 a, Vector3 b, Vector3 c)
        {
            float dot1, dot2;

            Vector3 n1, n2, n3, v1, v2, v3;

            v1 = a - point;
            v2 = b - point;
            v3 = c - point;

            n1 = Vector3.Cross(v1, v2);
            n2 = Vector3.Cross(v2, v3);
            n3 = Vector3.Cross(v3, v1);

            dot1 = Vector3.Dot(n1, n2);

            if (dot1 < 0) return false;

            dot2 = Vector3.Dot(n1, n3);

            if (dot2 < 0) return false;

            return true;
        }

        /// <summary>
        /// Punto mas cercano a una linea
        /// </summary>
        /// <param name="a">inicio del segmento de linea</param>
        /// <param name="b">final del segmento de linea</param>
        /// <param name="p">punto al que queremos encontrar el mas cercano</param>
        /// <returns>punto mas cercano en una linea</returns>
        public Vector3 ClosestPointOnLine(Vector3 a, Vector3 b, Vector3 p)
        {
            Vector3 v, c;
            float d, t;

            c = p - a;
            v = b - a;

            d = v.Length();

            v.Normalize();

            t = Vector3.Dot(v, c);

            // Check to see if ‘t’ is beyond the extents of the line segment

            if (t < 0) return a;
            if (t > d) return b;

            v *= t;

            return a + v;
        }

        /// <summary>
        /// Punto mas cercano a un triangulo
        /// </summary>
        /// <param name="a">primer vertice del triangulo</param>
        /// <param name="b">segundo vertice del triangulo</param>
        /// <param name="c">tercer vertice del triangulo</param>
        /// <param name="p">punto al que queremos encontrar el mas cercano</param>
        /// <returns>retorna el punto mas cercano en algun filo del triangulo</returns>
        public Vector3 ClosestPointOnTriangle(Vector3 a, Vector3 b, Vector3 c, Vector3 p)
        {
            Vector3 Rab, Rbc, Rca, temp;
            float dAB, dBC, dCA;

            Rab = ClosestPointOnLine(a, b, p);
            Rbc = ClosestPointOnLine(b, c, p);
            Rca = ClosestPointOnLine(c, a, p);

            temp = p - Rab;
            dAB = temp.Length();

            temp = p - Rbc;
            dBC = temp.Length();

            temp = p - Rca;
            dCA = temp.Length();

            if (dAB < dBC)
            {
                if (dAB < dCA) return Rab;
                else return Rca;
            }
            else
            {
                if (dBC < dCA) return Rbc;
                else return Rca;
            }
        }

        /// <summary>
        /// chequea si un punto esta en la esfera
        /// </summary>
        /// <param name="sphere"></param>
        /// <param name="p"></param>
        /// <returns></returns>
        public bool CheckPointInSphere(BoundingSphere sphere, Vector3 p)
        {
            ContainmentType result = sphere.Contains(p);

            return result == ContainmentType.Contains;
        }

        public bool CheckPointInSphere(Vector3 point, Vector3 sO, float sR)
        {
            Vector3 temp = point - sO;

            float d = temp.Length();

            if (d <= sR)
                return true;

            return false;
        }

        /// <summary>
        /// plano tangente a elipse
        /// </summary>
        /// <param name="point">punto en el que queremos calcular la normal</param>
        /// <param name="e0">origen de la elipse (centro)</param>
        /// <param name="eR">radio vector del elipse</param>
        /// <returns>un vector unitario tangente al plano deseado</returns>
        public Vector3 TangentPlaneNormalOfEllipsoid(Vector3 point, Vector3 e0, Vector3 eR)
        {
            Vector3 p = point - e0;

            float a2 = eR.X * eR.X;
            float b2 = eR.Y * eR.Y;
            float c2 = eR.Z * eR.Z;

            Vector3 result = new Vector3(p.X / a2, p.Y / b2, p.Z / c2);

            result.Normalize();

            return result;
        }

        public enum PosicionRespectoPlano
        {
            FRONT,
            BACKSIDE,
            ONPLANE
        }

        /// <summary>
        /// Clasificar punto respecto al plano
        /// </summary>
        /// <param name="point"></param>
        /// <param name="p0">punto en el plano</param>
        /// <param name="pN">Vector normal al plano</param>
        /// <returns>PosicionRespectoPlano</returns>
        public PosicionRespectoPlano ClassifyPoint(Vector3 point, Vector3 p0, Vector3 pN)
        {
            Vector3 dir = p0 - point;

            float d = Vector3.Dot(dir, pN);

            if (d < -0.001f) return PosicionRespectoPlano.FRONT;

            if (d > 0.001f) return PosicionRespectoPlano.BACKSIDE;

            return PosicionRespectoPlano.ONPLANE;
        }

        /// <summary>
        /// chequea una colision contra una serie de poligonos
        /// </summary>
        /// <param name="colPackage">estructura de colision actualizada</param>
        public void CheckCollision()
        {
            colPackage.foundCollision = false;

            float kk = IntersectRayPlane(new Vector3(0, 0, -0.1f), new Vector3(0, 0, 1), new Vector3(-0.5f, -0.5f, 0), new Vector3(0, 0, 1));
            kk=IntersectRayPlane(new Ray(new Vector3(0, 0, -0.1f), new Vector3(0, 0, 1)),new Plane(new Vector3(0,0,1),0));
            // plane data
            Vector3 p1, p2, p3;
            Vector3 pNormal;
            Vector3 pOrigin;

            // datos iniciales
            Vector3 source = colPackage.sourcePoint;
            Vector3 velocity = colPackage.velocity;

            // se normaliza y se guarda
            Vector3 normalizedVelocity = Vector3.Normalize(velocity);

            // datos de interseccion
            Vector3 sIPoint;    // interseccion en la esfera
            Vector3 pIPoint;    // punto de interseccion en el plano
            Vector3 polyIPoint; // punto de interseccion en el poligono

            // longitud de nuestra velocidad
            float distanceToTravel = velocity.Length();

            float distToPlaneIntersection;
            float distToEllipsoidIntersection;

            foreach (Triangle3 triangle in Triangles)
            {
                // toma los datos del triangulo y los transforma a espacio elipsodial
                p1 = triangle.A / eRadius;
                p2 = triangle.B / eRadius;
                p3 = triangle.C / eRadius;

                // crea el plano que contiene el triangulo
                pOrigin = p1;
                Vector3 v1, v2;
                v1 = p2 - p1;
                v2 = p3 - p1;

                // determina la normal de dicho plano
                pNormal = Vector3.Cross(v2, v1); // Atención,m aquí he cambiado el orden para que la normal sea correcta
                pNormal.Normalize();

                //No puedo usar la normal, pq cambiar al pasar al espacio elipsoidal 
                // pNormal=pt->normal;
                // calcula el punto de interseccion en la esfera
                sIPoint = source - pNormal;

                // clasifica el punto para saber si el plano atraviesa a la esfera
                PosicionRespectoPlano pClass = ClassifyPoint(sIPoint, pOrigin, pNormal);

                // encuentra el punto de interseccion en el plano
                if (pClass == PosicionRespectoPlano.BACKSIDE)
                { // la esfera esta seccionada con el plano

                    // encuentra un punto de interseccion trazando un rayo desde el
                    // punto de interseccion en la esfera tangente al plano
                    distToPlaneIntersection = IntersectRayPlane(sIPoint, pNormal, pOrigin, pNormal);

                    // calcula el punto de interseccion en el plano
                    pIPoint = sIPoint + pNormal * distToPlaneIntersection;

                }
                else
                {

                    // traza un rayo a lo largo de la velocidad
                    distToPlaneIntersection = IntersectRayPlane(sIPoint, normalizedVelocity, pOrigin, pNormal);

                    // calcula el punto de interseccion en el plano
                    pIPoint = sIPoint + normalizedVelocity * distToPlaneIntersection;

                }

                // encuentra el punto de interseccion en el poligono, por defecto asume que
                // este es igual al punto de interseccion en el plano
                polyIPoint = pIPoint;
                distToEllipsoidIntersection = distToPlaneIntersection;

                // chequea si el punto esta dentro del triangulo
                if (!CheckPointInTriangleNormal(pIPoint, p1, p2, p3))
                {

                    // si no esta en el triangulo
                    polyIPoint = ClosestPointOnTriangle(p1, p2, p3, pIPoint);

                    distToEllipsoidIntersection = IntersectRaySphere(polyIPoint, -normalizedVelocity, source, 1.0f);

                    if (distToEllipsoidIntersection > 0.0f)
                    {
                        // calculate true sphere intersection point
                        sIPoint = polyIPoint - distToEllipsoidIntersection * normalizedVelocity;
                    }
                }

                // se realiza un chequeo para ver si el jugador queda atrapado
                if (CheckPointInSphere(polyIPoint, source, 1.0f))
                    colPackage.stuck = true;
                
                // ahora debemos actualizar los datos de la colision
                // He cambiado el >0.0f por el >=, pq todavía no trato el stuck=TRUE
                if ((distToEllipsoidIntersection >= 0.0f) && (distToEllipsoidIntersection <= distanceToTravel))
                {
                    if ((colPackage.foundCollision == false) || (distToEllipsoidIntersection < colPackage.nearestDistance))
                    {

                        // si tenemos una colision mas cerca guardamos la informacion
                        colPackage.nearestDistance = distToEllipsoidIntersection;
                        colPackage.nearestIntersectionPoint = sIPoint;
                        colPackage.nearestPolygonIntersectionPoint = polyIPoint;
                        colPackage.foundCollision = true;

                        HayColision = true;
                    }
                }
            } // continua con la siguiente cara (triangulo)
        }

        /// <summary>
        /// funcion principal para deteccion de colisiones
        /// </summary>
        /// <param name="position"></param>
        /// <param name="velocity"></param>
        /// <returns></returns>
        public Vector3 GetPosition(Vector3 position, Vector3 velocity)
        {

            Vector3 scaledPosition, scaledVelocity;

            HayColision = false;

            // lo primero es transformar la posicion del jugador y su velocidad
            // a espacio elipsoidal...
            scaledPosition = position / eRadius;
            scaledVelocity = velocity / eRadius;

            colPackage.lastSafePosition = scaledPosition;
            // invoca a la funcion recursiva de deteccion de colisiones
            Vector3 resul = CollideWithWorld(scaledPosition, scaledVelocity);

            // cuando la funcion retorna un resultado, este sigue en espacio elipsodial
            // entonces transformamos a espacio normal.

            return resul * eRadius;
        }

        //********************************************************************************
        // funcion recursiva, procesa una colision y responde adecuadamente a la misma
        //********************************************************************************
        public Vector3 CollideWithWorld(Vector3 position, Vector3 velocity)
        {

            Vector3 destinationPoint, V, newSourcePoint, newDestinationPoint, newVelocityVector;
            float velocityLenght, l;
            float EPSILON = 0.0001f;

            // si la velocidad es menor a epsilon no nos preocupamos ?
            velocityLenght = velocity.Length();
            if (velocityLenght < EPSILON) return position;

            destinationPoint = position + velocity;

            // reseteamos la estructura con info de colisiones
            colPackage.velocity = velocity;
            colPackage.sourcePoint = position;
            colPackage.foundCollision = false;
            colPackage.stuck = false;
            colPackage.nearestDistance = -1;

            //############ detecta una colision ##################
            CheckCollision();
            //####################################################

            // chequea el valor de retorno y posiblemente invoca recursivamente
            if (colPackage.foundCollision == false)
            {
                // si  ninguna colision fue detectada se mueve muy cerca de la posicion deseada
                V = velocity;
                V = SetLength(V, velocityLenght - EPSILON);

                // actualiza la ultima posicion A SALVO (sin colision)
                colPackage.lastSafePosition = position;

                // retorna la posicion final
                return position + V;
            }
            else
            {
                // si hubo una colision

                // si queda atrapado, solo retorna la ultima posicion A SALVO
                /*
                 * if (colPackage.stuck)
                    return colPackage.lastSafePosition;
                */

                // lo primero es moverse hasta el punto donde colisiono con algo
                // solo actualiza si no esta muy cerca
                if (colPackage.nearestDistance >= EPSILON)
                {
                    V = velocity;
                    V = SetLength(V, colPackage.nearestDistance - EPSILON);
                    newSourcePoint = colPackage.sourcePoint + V;
                }
                else
                    newSourcePoint = colPackage.sourcePoint;


                // ahora debemos calcular el plano de deslizamiento
                Vector3 slidePlaneOrigin = colPackage.nearestPolygonIntersectionPoint;
                Vector3 slidePlaneNormal = newSourcePoint - colPackage.nearestPolygonIntersectionPoint;

                // ahora proyectamos el punto deseado en el plano de deslizamiento
                l = IntersectRayPlane(destinationPoint, slidePlaneNormal,
                                         slidePlaneOrigin, slidePlaneNormal);

                // ahora podemos calcular un nuevo punto de destino
                newDestinationPoint = destinationPoint + slidePlaneNormal * l;


                // genera un nuevo vector de deslizameinto que se transforma en nuestra
                // nueva velocidad
                newVelocityVector = newDestinationPoint - colPackage.nearestPolygonIntersectionPoint;


                // ahora recursivamente podemos llamar a la funcion
                //collision->lastSafePosition = *position;

                return CollideWithWorld(newSourcePoint, newVelocityVector);
            }

        }

        /// <summary>
        /// Establece la longitud de un vector
        /// </summary>
        /// <param name="v"></param>
        /// <param name="l">Longitud final</param>
        /// <returns></returns>
        Vector3 SetLength(Vector3 v, float l)
        {
            float len = v.Length();

            v *= (l / len);

            return v;
        }
    }
#else

    public class CollisionPacket
    {

        // data about player movement
        public Vector3 velocity;
        public Vector3 sourcePoint;
        
        public Vector3 normalizedVelocity;
        public Vector3 R3Position;
        public Vector3 R3Velocity, intersectionPoint, basePoint;
        public Triangle3 collisionTri=new Triangle3();
        public float t;
        // radius of ellipsoid.  
        //D3DXVECTOR3 eRadius;

        // for error handling  
        public Vector3 lastSafePosition;
        public bool stuck;

        // data for collision response 
        public bool foundCollision;
        public float nearestDistance; // nearest distance to hit
        /// <summary>
        /// on sphere
        /// </summary>
        public Vector3 nearestIntersectionPoint;
        public Vector3 nearestPolygonIntersectionPoint; // on polygon
    };

    public class tPlane
    {
        public Vector3 Point;
        public Vector3 normal;
        public float d;

        public tPlane(Vector3 a, Vector3 b, Vector3 c)
        {
            Point = a;

            Vector3 v1, v2;
            v1 = b - a;
            v2 = c - a;

            // determina la normal de dicho plano
            normal = Vector3.Cross(v2, v1); // Atención,m aquí he cambiado el orden para que la normal sea correcta
            normal.Normalize();

            Build();
        }

        public tPlane(Vector3 pto,Vector3 normal)
        {
            Point=pto;
            this.normal=normal;

            Build();
        }

        void Build()
        {
            d = -Vector3.Dot(normal, Point);
        }

        public float signedDistanceTo(Vector3 pto)
        {

            float t = Vector3.Dot(normal, pto) + d;

            return t;
        }

        public bool isFrontFacingTo(Vector3 pto)
        {
            return signedDistanceTo(pto) > 0;
        }

    }

    public class Collision
    {
        /// <summary>
        /// Lista de triángulos que se comprueba con la colisión
        /// </summary>
        public List<Triangle3> Triangles;

        public List<Triangle3> GroundTriangles;

        /// <summary>
        /// Información sobre el resultado de la colisión
        /// </summary>
        public CollisionPacket collisionPackage = new CollisionPacket();

        /// <summary>
        /// Radio de la elipsoide
        /// </summary>
        public Vector3 eRadius = new Vector3(1.5f, 0.5f, 1.5f)*1.0f;

        //public Vector3 eRadius = new Vector3(1,1,1);

        public bool HayColision;

        private bool GetLowestRoot(float a, float b, float c, float maxR, ref float root)
        {
            
            // Check if a solution exists
            float determinant = b * b - 4.0f * a * c;

            // If determinant is negative it means no solutions.
            if (determinant < 0.0f) return false;

            // calculate the two roots: (if determinant == 0 then
            // x1==x2 but let�s disregard that slight optimization)
            float sqrtD = (float)Math.Sqrt(determinant);
            float r1 = (-b - sqrtD) / (2 * a);
            float r2 = (-b + sqrtD) / (2 * a);

            // Sort so x1 <= x2
            if (r1 > r2)
            {
                float temp = r2;
                r2 = r1;
                r1 = temp;
            }

            // Get lowest root:
            if (r1 > 0 && r1 < maxR)
            {
                root = r1;
                return true;
            }

            // It is possible that we want x2 - this can happen
            // if x1 < 0
            if (r2 > 0 && r2 < maxR)
            {
                root = r2;
                return true;
            }

            // No (valid) solutions
            return false;
        }


        /// <summary>
        /// Check point P to see if it is within the triangle A B C.
        /// </summary>
        private bool CheckPointInTriangle(Vector3 P, Vector3 A, Vector3 B, Vector3 C)
        {
            //Distance Vectors        
            Vector3 p1 = C - A;
            Vector3 p2 = B - A;
            Vector3 p3 = P - A;

            //Get all the Vector Dots
            float dot11 = Vector3.Dot(p1, p1);
            float dot12 = Vector3.Dot(p1, p2);
            float dot13 = Vector3.Dot(p1, p3);
            float dot22 = Vector3.Dot(p2, p2);
            float dot23 = Vector3.Dot(p2, p3);

            //Barycentric Co-ordinates
            float invDenom = 1 / (dot11 * dot22 - dot12 * dot12);
            float u = (dot22 * dot13 - dot12 * dot23) * invDenom;
            float v = (dot11 * dot23 - dot12 * dot13) * invDenom;

            // Is 'P' in the Triangle?
            return (u > 0) && (v > 0) && (u + v < 1);
        }


        /// <summary>
        /// Check a triangle for collision. Assumes p1,p2 and p3 are given in ellipsoid space.
        /// </summary>
        private void CheckTriangle(CollisionPacket colPackage, Vector3 p1, Vector3 p2, Vector3 p3)
        {
            // Make the plane containing this triangle.
            tPlane trianglePlane = new tPlane(p1, p2, p3);

            // Is triangle front-facing to the velocity vector?
            // We only check front-facing triangles
            // (your choice of course)

            //if (trianglePlane.isFrontFacingTo(colPackage.normalizedVelocity))
            if (true)
            {
                // Get interval of plane intersection:
                double t0, t1;
                bool embeddedInPlane = false;

                // Calculate the signed distance from sphere
                // position to triangle plane
                double signedDistToTrianglePlane = trianglePlane.signedDistanceTo(colPackage.basePoint);

                // cache this as we�re going to use it a few times below:
                float normalDotVelocity = Vector3.Dot(trianglePlane.normal, colPackage.velocity);

                // if sphere is travelling parrallel to the plane:
                if (normalDotVelocity == 0.0f)
                {
                    if (Math.Abs(signedDistToTrianglePlane) >= 1.0f)
                    {
                        // Sphere is not embedded in plane.
                        // No collision possible:
                        return;
                    }
                    else
                    {
                        // sphere is embedded in plane.
                        // It intersects in the whole range [0..1]
                        embeddedInPlane = true;
                        t0 = 0.0;
                        t1 = 1.0;
                    }
                }
                else
                {
                    // N dot D is not 0. Calculate intersection interval:
                    t0 = (-1.0 - signedDistToTrianglePlane) / normalDotVelocity;
                    t1 = (1.0 - signedDistToTrianglePlane) / normalDotVelocity;

                    // Swap so t0 < t1
                    if (t0 > t1)
                    {
                        double temp = t1;
                        t1 = t0;
                        t0 = temp;
                    }

                    // Check that at least one result is within range:
                    if (t0 > 1.0f || t1 < 0.0f)
                    {
                        // Both t values are outside values [0,1]
                        // No collision possible:
                        return;
                    }

                    // Clamp to [0,1]
                    if (t0 < 0.0) t0 = 0.0;
                    if (t1 < 0.0) t1 = 0.0;
                    if (t0 > 1.0) t0 = 1.0;
                    if (t1 > 1.0) t1 = 1.0;
                }

                // OK, at this point we have two time values t0 and t1
                // between which the swept sphere intersects with the
                // triangle plane. If any collision is to occur it must
                // happen within this interval.
                Vector3 collisionPoint = new Vector3();
                bool foundCollison = false;
                float t = 1.0f;

                // First we check for the easy case - collision inside
                // the triangle. If this happens it must be at time t0
                // as this is when the sphere rests on the front side
                // of the triangle plane. Note, this can only happen if
                // the sphere is not embedded in the triangle plane.
                if (!embeddedInPlane)
                {
                    Vector3 planeIntersectionPoint;

                    planeIntersectionPoint =
                        (trianglePlane.isFrontFacingTo(colPackage.normalizedVelocity)) ?
                            (colPackage.basePoint - trianglePlane.normal) :
                            (colPackage.basePoint + trianglePlane.normal);
                    planeIntersectionPoint += Vector3.Multiply(colPackage.velocity, (float)t0);



                    if (CheckPointInTriangle(planeIntersectionPoint, p1, p2, p3))
                    {
                        foundCollison = true;
                        t = (float)t0;
                        //myDebug.write(trianglePlane.isFrontFacingTo(colPackage.normalizedVelocity).ToString() + " - " + colPackage.normalizedVelocity.ToString());
                        collisionPoint = planeIntersectionPoint;
                        //myDebug.writeVector("Found collision in a plane: ", planeIntersectionPoint * colPackage.eRadius);
                        //myDebug.writeVector("Base Point: ", colPackage.basePoint * colPackage.eRadius);
                        colPackage.collisionTri.A = p1;
                        colPackage.collisionTri.B = p2;
                        colPackage.collisionTri.C = p3;
                    }
                }

                // if we haven�t found a collision already we�ll have to
                // sweep sphere against points and edges of the triangle.
                // Note: A collision inside the triangle (the check above)
                // will always happen before a vertex or edge collision!
                // This is why we can skip the swept test if the above
                // gives a collision!
                if (foundCollison == false)
                {
                    // some commonly used terms:
                    Vector3 velocity = colPackage.velocity;
                    Vector3 basePoint = colPackage.basePoint;
                    float velocitySquaredLength = velocity.LengthSquared();
                    float a, b, c; // Params for equation
                    float newT = 0.0f;

                    // For each vertex or edge a quadratic equation have to
                    // be solved. We parameterize this equation as
                    // a*t^2 + b*t + c = 0 and below we calculate the
                    // parameters a,b and c for each test.
                    // Check against points:
                    a = velocitySquaredLength;

                    // P1
                    b = 2 * Vector3.Dot(velocity, basePoint - p1);
                    c = (p1 - basePoint).LengthSquared() - 1.0f;
                    if (GetLowestRoot(a, b, c, t, ref newT))
                    {
                        t = newT;
                        foundCollison = true;
                        collisionPoint = p1;
                    }

                    // P2
                    b = 2 * Vector3.Dot(velocity, basePoint - p2);
                    c = (p2 - basePoint).LengthSquared() - 1.0f;
                    if (GetLowestRoot(a, b, c, t, ref newT))
                    {
                        t = newT;
                        foundCollison = true;
                        collisionPoint = p2;
                    }

                    // P3
                    b = 2 * Vector3.Dot(velocity, basePoint - p3);
                    c = (p3 - basePoint).LengthSquared() - 1.0f;
                    if (GetLowestRoot(a, b, c, t, ref newT))
                    {
                        t = newT;
                        foundCollison = true;
                        collisionPoint = p3;
                    }

                    //------------------------------
                    // Check agains edges:
                    //------------------------------

                    // p1 -> p2:
                    //------------------------------
                    Vector3 edge = p2 - p1;
                    Vector3 baseToVertex = p1 - basePoint;
                    float edgeSquaredLength = edge.LengthSquared();
                    float edgeDotVelocity = Vector3.Dot(edge, velocity);
                    float edgeDotBaseToVertex = Vector3.Dot(edge, baseToVertex);

                    // Calculate parameters for equation
                    a = (edgeSquaredLength * -velocitySquaredLength) + (edgeDotVelocity * edgeDotVelocity);
                    b = edgeSquaredLength * (2 * Vector3.Dot(velocity, baseToVertex)) - (2 * edgeDotVelocity * edgeDotBaseToVertex);
                    c = edgeSquaredLength * (1 - baseToVertex.LengthSquared()) + (edgeDotBaseToVertex * edgeDotBaseToVertex);

                    // Does the swept sphere collide against infinite edge?
                    if (GetLowestRoot(a, b, c, t, ref newT))
                    {
                        // Check if intersection is within line segment:
                        float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
                        if (f >= 0.0 && f <= 1.0)
                        {
                            // intersection took place within segment.
                            t = newT;
                            foundCollison = true;
                            collisionPoint = p1 + (f * edge);
                        }
                    }

                    // p2 -> p3:
                    //------------------------------
                    edge = p3 - p2;
                    baseToVertex = p2 - basePoint;
                    edgeSquaredLength = edge.LengthSquared();
                    edgeDotVelocity = Vector3.Dot(edge, velocity);
                    edgeDotBaseToVertex = Vector3.Dot(edge, baseToVertex);

                    a = (edgeSquaredLength * -velocitySquaredLength) + (edgeDotVelocity * edgeDotVelocity);
                    b = edgeSquaredLength * (2 * Vector3.Dot(velocity, baseToVertex)) - (2.0f * edgeDotVelocity * edgeDotBaseToVertex);
                    c = edgeSquaredLength * (1 - baseToVertex.LengthSquared()) + (edgeDotBaseToVertex * edgeDotBaseToVertex);

                    if (GetLowestRoot(a, b, c, t, ref newT))
                    {
                        float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
                        if (f >= 0.0 && f <= 1.0)
                        {
                            t = newT;
                            foundCollison = true;
                            collisionPoint = p2 + (f * edge);
                        }
                    }

                    // p3 -> p1:
                    //------------------------------
                    edge = p1 - p3;
                    baseToVertex = p3 - basePoint;
                    edgeSquaredLength = edge.LengthSquared();
                    edgeDotVelocity = Vector3.Dot(edge, velocity);
                    edgeDotBaseToVertex = Vector3.Dot(edge, baseToVertex);

                    a = edgeSquaredLength * -velocitySquaredLength + (edgeDotVelocity * edgeDotVelocity);
                    b = edgeSquaredLength * (2 * Vector3.Dot(velocity, baseToVertex)) - (2.0f * edgeDotVelocity * edgeDotBaseToVertex);
                    c = edgeSquaredLength * (1 - baseToVertex.LengthSquared()) + (edgeDotBaseToVertex * edgeDotBaseToVertex);

                    if (GetLowestRoot(a, b, c, t, ref newT))
                    {
                        float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
                        if (f >= 0.0 && f <= 1.0)
                        {
                            t = newT;
                            foundCollison = true;
                            collisionPoint = p3 + (f * edge);
                        }
                    }
                }//Emd If Collision Found

                //------------------------------
                // Set result:
                //------------------------------
                if (foundCollison == true)
                {
                    // distance to collision: �t� is time of collision
                    float distToCollision = t * colPackage.velocity.Length();

                    // Does this triangle qualify for the closest hit?
                    // it does if it�s the first hit or the closest
                    if (colPackage.foundCollision == false || distToCollision < colPackage.nearestDistance)
                    {

                        // Collision information nessesary for sliding
                        colPackage.nearestDistance = distToCollision;
                        colPackage.intersectionPoint = collisionPoint;
                        colPackage.foundCollision = true;
                        colPackage.t = t;
                    }
                }
            }//End if not backface
        }
        
        public int collisionRecursionDepth;
        bool planesUpdated = false;

        void UpdatePlane()
        {
            foreach (Triangle3 triangle in GroundTriangles)
            {
                triangle.Plane = new Plane(triangle.A, triangle.B, triangle.C);
            }

            planesUpdated = true;
        }

        Vector3 CollideWithGround(Vector3 pos)
        {
            if(!planesUpdated) UpdatePlane();

            Ray ray=new Ray(pos,Vector3.Down);

            foreach (Triangle3 triangle in GroundTriangles)
            {
                // Calculamos la colisión de la recta con el plano que forma el triángulo
                /*
                Plane p=new Plane(triangle.A,triangle.B,triangle.C);

                if (triangle.Plane == null)
                {
                    triangle.Plane = new Plane(triangle.A, triangle.B, triangle.C);
                }
                */
                float? d = ray.Intersects(triangle.Plane);

                if(d.HasValue)
                {
                    Vector3 pointInPlane=ray.Position+ray.Direction*d.Value;

                    if(CheckPointInTriangle(pointInPlane,triangle.A,triangle.B,triangle.C))
                    {
                        return pointInPlane;
                    }
                }
            }

            return pos;
        }

        /// <summary>
        /// 
        /// </summary>
        private Vector3 CollideWithWorld(Vector3 pos, Vector3 vel)
        {

            float unitsPerMeter = 10.0f; //Set this to match application scale.
            // All hard-coded distances in this function is
            // scaled to fit the setting above..
            float unitScale = unitsPerMeter / 100.0f;
            float veryCloseDistance = 0.005f * unitScale;

            // do we need to worry?
            if (collisionRecursionDepth > 5)
                return pos;

            // Ok, we need to worry:
            collisionPackage.velocity = vel;
            collisionPackage.normalizedVelocity = vel;
            collisionPackage.normalizedVelocity.Normalize();
            collisionPackage.basePoint = pos;
            collisionPackage.foundCollision = false;

            //Check all triangles for collision.
            foreach (Triangle3 t in Triangles)
            {
                CheckTriangle(collisionPackage,
                                        t.A / eRadius,
                                        t.B / eRadius,
                                        t.C / eRadius);
                if (collisionPackage.foundCollision)
                {
                    //Console.WriteLine("Collision Found! "+ collisionPackage.intersectionPoint.ToString());
                    /*
                    collisionPoint.tri = collisionPackage.collisionTri;
                    collisionPoint.tri.v1 *= collisionPackage.eRadius;
                    collisionPoint.tri.v2 *= collisionPackage.eRadius;
                    collisionPoint.tri.v3 *= collisionPackage.eRadius;
                    collisionPoint.colpack = collisionPackage;
                    */
                    //break;

                }
            }

            // If no collision we just move along the velocity
            if (collisionPackage.foundCollision == false)
            {
                return pos + vel;
            }

            HayColision = true;

            // *** Collision occured ***
            // The original destination point
            Vector3 destinationPoint = pos + vel;
            Vector3 newBasePoint = pos;

            // only update if we are not already very close
            // and if so we only move very close to intersection..not
            // to the exact spot.

            if (collisionPackage.nearestDistance >= veryCloseDistance)
            {
                Vector3 V = vel;
                V.Normalize();
                V = Vector3.Multiply(V, (float)(collisionPackage.nearestDistance - veryCloseDistance));
                //V.SetLength = (collisionPackage.nearestDistance - veryCloseDistance);

                newBasePoint = collisionPackage.basePoint + V;

                // Adjust polygon intersection point (so sliding
                // plane will be unaffected by the fact that we
                // move slightly less than collision tells us)
                V.Normalize();
                collisionPackage.intersectionPoint -= (veryCloseDistance * V);
            }

            // Determine the sliding plane
            Vector3 slidePlaneOrigin = collisionPackage.intersectionPoint;
            Vector3 slidePlaneNormal = newBasePoint - collisionPackage.intersectionPoint;
            slidePlaneNormal.Normalize();
            tPlane slidingPlane = new tPlane(slidePlaneOrigin, slidePlaneNormal);

            // Again, sorry about formatting.. but look carefully ;)
            Vector3 newDestinationPoint = destinationPoint - Vector3.Multiply(slidePlaneNormal, (float)slidingPlane.signedDistanceTo(destinationPoint));

            // Generate the slide vector, which will become our new velocity vector for the next iteration
            Vector3 newVelocityVector = newDestinationPoint - collisionPackage.intersectionPoint;

            // Recurse: Don't recurse if the new velocity is very small
            if (newVelocityVector.Length() < veryCloseDistance)
            {
                return newBasePoint;
            }

            collisionRecursionDepth++;

            return CollideWithWorld(newBasePoint, newVelocityVector);
        }


        /// <summary>
        /// Conversion to eSpace for collideWithWorld.
        /// </summary>
        public Vector3 CollideAndSlide(Vector3 pos, Vector3 vel, Vector3 gravity)
        {


            // Do collision detection:
            collisionPackage.R3Position = pos;
            //collisionPackage.R3Position.Y += eRadius.Y;
            collisionPackage.R3Velocity = vel;

            // calculate position and velocity in eSpace
            Vector3 eSpacePosition = collisionPackage.R3Position / eRadius;
            Vector3 eSpaceVelocity = collisionPackage.R3Velocity / eRadius;

            // Iterate until we have our final position.
            collisionRecursionDepth = 0;
            Vector3 finalPosition = CollideWithWorld(eSpacePosition, eSpaceVelocity);

            // [gravity]
            if (gravity.Y != 0)
            {
                collisionPackage.R3Position = finalPosition * eRadius;
                collisionPackage.R3Velocity = gravity;
                eSpaceVelocity = gravity / eRadius;
                collisionRecursionDepth = 0;
                //finalPosition = CollideWithWorld(finalPosition, eSpaceVelocity);

                // Convert final result back to R3:
                finalPosition = finalPosition * eRadius;

                finalPosition = CollideWithGround(finalPosition) + new Vector3(0, 10.0f, 0);

                HayColision = true;
            }
            // [/gravity]
            else
                // Convert final result back to R3:
                finalPosition = finalPosition * eRadius;

           // finalPosition.Y -= eRadius.Y;

            // Move the entity (application specific function)
            return finalPosition;
        }

        public Vector3 GetPosition(Vector3 position, Vector3 velocity,float gravity=-1.5f)
        {

            HayColision = false;

            // Comprobaré si me salgo de los límites del mundo

            // invoca a la funcion recursiva de deteccion de colisiones
            Vector3 resul = CollideAndSlide(position, velocity, new Vector3(0, gravity, 0));


            if (resul.X < -166 || resul.X > 166 || resul.Z < -184 || resul.Z > 219) resul = position;

            return resul;
        }
    }

#endif

}
